#!/usr/bin/env python

""" nav_test.py - Version 1.1 2013-12-20

    Command a robot to move autonomously among a number of goal locations defined in the map frame.
    On each round, select a new random sequence of locations, then attempt to move to each location
    in succession.  Keep track of success rate, time elapsed, and total distance traveled.

    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2012 Patrick Goebel.  All rights reserved.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.5
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:
    
    http://www.gnu.org/licenses/gpl.html
      
"""
import re
import random
import rospy
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from random import sample
from math import pow, sqrt

def is_float(s):
	value = re.compile(r'^[-+]?[0-9]+\.[0-9]+$')
	result = value.match(s)
	if result:
		return True
	else:
		return False

def is_int(s):
 	try:
		int(s)
		return True
	except ValueError:
		return False

def mutation_by_type(cell):
	if cell[2]=='true':
		res=[cell[0],cell[1],'false']
		stype='bool'
	elif cell[2]=='false':
		res=[cell[0],cell[1],'true']
		stype='bool'
	elif is_int(cell[2]):
		rint=random.randint(0,100)
		res=[cell[0],cell[1],str(rint)]
		stype='int'
	elif is_float(cell[2]):
		rfloat=random.uniform(0, 100)
		res=[cell[0],cell[1],str(rfloat)]
		stype='float'
	else:
		res=cell
		stype='string'
	return res
		

def read_result(path,yaml_path):
	with open(path,"r") as file:
		lines=file.readlines()
	length=[]
	steps=[]
	time=[]
	yamls=[]
	for i in range(len(lines)):
		line=lines[i]
		sp=line.split('\t')
		#print(sp)
		if len(sp)==3:
			single_yaml=read_single_yaml(yaml_path,i)
			length.append(float(sp[0].strip()))
			steps.append(float(sp[1].strip()))
			time.append(int(sp[2].strip()))
			yamls.append(single_yaml)
			#print(i)
		#else:
			#length.append(12.0)
			#steps.append(500.0)
			#time.append(100.0)
	return length,steps,time,yamls
    
def fitness(length,steps,time):
	
	list_norm(length)
	list_norm(steps)
	list_norm(time)
	#print(length,steps,time)
	fit_l=[]
	for i in range(len(length)):
		l=length[i]
		s=steps[i]
		t=time[i]
		fit=0.4*l+0.2*s+0.4*t
		#fit=t
		#print('fitness'+str(fit))
		fit_l.append(fit)
	list_norm(fit_l)
	return fit_l

      
def list_norm(l):
	max_l=max(l)
	min_l=min(l)
	range_l=max_l-min_l
	for i in range(len(l)):
		l[i]=(l[i]-min_l)/range_l

def read_yaml(path,n):
	yamls=[]
	for i in range(n):
		single_yaml=read_single_yaml(path,i)
		yamls.append(single_yaml)
	return yamls

def read_single_yaml(path,index):
	single_yaml=[]
	with open(path+'/'+str(index)+'.yaml',"r") as file:
		lines=file.readlines()
		for line in lines:
			line=line.strip()
			sp=line.split(':')
			if(len(sp)==2 and len(sp[1])>0):
				cell=['param',sp[0].strip(),sp[1].strip()]
				single_yaml.append(cell)
				#print(cell)
			else:
				cell=['namespace',sp[0].strip(),'']
				single_yaml.append(cell)
				#print(cell)
	return single_yaml
def write_yaml(path,yamls):
	for i in range(len(yamls)):
		yaml=yamls[i]
		with open(path+'/'+str(i)+'.yaml',"w") as file:
			for line in yaml:
				if line[0]=='namespace':
					file.write(line[1]+':\n')
				else:
					file.write(' '+line[1]+': '+line[2]+'\n')
def convert_to_prop(fitness):
	s=sum(fitness)
	prop=[]
	for i in range(len(fitness)):
		prop.append(fitness[i]/s)
	return 	prop

def choose(yamls,fitness,choose_n):
	result=[]
	prop=convert_to_prop(fitness)
	#print(prop)
	for i in range(len(prop)):
		r=random.random()
		if r > prop[i]:
			result.append(yamls[i])
			if(len(result)==choose_n):
				break
	
	return result

def cross(yaml1,yaml2):
	rf=0.5
	result=[]
	for i in range(len(yaml1)):
		if yaml1[i][0]=='param':
			r=random.random()
			if r < rf:
				result.append(yaml2[i])
			else:
				result.append(yaml1[i])
		else:
			result.append(yaml1[i])
	return result

def mutation(yaml):
	rf=0.5
	result=[]
	for i in range(len(yaml)):
		if yaml[i][0]=='param':
			r=random.random()
			if r < rf:
				res=mutation_by_type(yaml[i])
				result.append(res)
			else:
				result.append(yaml[i])
		else:
			result.append(yaml[i])
	
	return result

def gen_cross(parents,cross_n):
	result=[]
	for i in range(cross_n):
		index1=random.randint(0,len(parents)-1)
		index2=random.randint(0,len(parents)-1)
		if index1 != index2:
			res=cross(parents[index1],parents[index2])
			result.append(res)
		else:
			if index1==len(parents)-1:
				index2=index1-1
			else:
				index2=index1+1
			
			res=cross(parents[index1],parents[index2])
			result.append(res)
	return result

def gen_mutation(parents,mutation_n):
	result=[]
	for i in range(mutation_n):
		index=random.randint(0,len(parents)-1)
		res=mutation(parents[index])
		result.append(res)
	return result
	

def next_gen(parents):
	print('choose parents number:'+str(len(parents))+'\n')
	print('gen next generation...')
	gen=[]
	cross_r=0.5
	gen_n=10-len(parents)

	cross_n=gen_n//(1/cross_r)
	mutation_n=gen_n-cross_n

	print('cross_number:'+str(cross_n)+'\tmutation_number:'+str(mutation_n))
	cross=gen_cross(parents,int(cross_n))
	mutation=gen_mutation(parents,int(mutation_n))
	
	
	gen.extend(cross)
	gen.extend(mutation)
	
	print('gen children:'+str(len(gen)))
	print('complete gen next generation.\n')
	return gen
	
	
					

if __name__ == '__main__':
    	n=10
	parent_path='/home/nero/yaml_gen'
	next_gen_path='/home/nero/yaml_gen'
	res_path='/home/nero/res.txt'
	length,steps,time,yamls=read_result(res_path,parent_path)
	fit=fitness(length,steps,time)
	#yamls=read_yaml(parent_path,n)
	parents=choose(yamls,fit,4)
	print(len(fit),len(yamls))
	next_yamls=next_gen(parents)

	next_yamls.extend(parents)

	print('writing to yamls...')
	write_yaml(next_gen_path,next_yamls)
	print('complete writing to yamls.\n')
	print('finish gen next generation.')
	#write_yaml(next_gen_path,yamls)
	#print(yamls)
 
    
